#!/usr/bin/env python

import rospy
import roslib
import os
import actionlib
from std_msgs.msg import Float32
from geometry_msgs.msg import Twist 
from move_base_msgs.msg import MoveBaseActionResult,MoveBaseGoal,MoveBaseActionGoal,MoveBaseAction
from actionlib_msgs.msg import *

global send_goal,goal
send_goal=MoveBaseActionGoal()
goal=MoveBaseGoal()
def goal_status(status):
    return {GoalStatus.PENDING:'PENDING',
            GoalStatus.ACTIVE:'ACTIVE',
            GoalStatus.PREEMPTED:'PREEMPTED',
            GoalStatus.SUCCEEDED:'SUCCEEDED',
            GoalStatus.ABORTED:'ABORTED',
            GoalStatus.REJECTED:'EJECTED',
            GoalStatus.RECALLING:'RECALLING',
            GoalStatus.RECALLED:'RECALLED',
            GoalStatus.LOST:'LOST'}[status]

class NavState:

    def __init__(self):
        rospy.init_node("nav_state")
        nodename = rospy.get_name()
        rospy.loginfo("%s started" % nodename)
        self.move_base=actionlib.SimpleActionClient("move_base",MoveBaseAction) 
        rospy.Subscriber('/move_base/goal', MoveBaseActionGoal, self.GoalCallback,queue_size=1)   
        rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.ResultCallback,queue_size=1) 
        self.move_base.wait_for_server(rospy.Duration(60))
        self.rate = rospy.get_param("~rate", 50)  
        self.goal_num = rospy.get_param("~repeat_num", 1)  
        self.count=1                                        

    def ResultCallback(self,msg):
        text = "{}: {}".format(goal_status(msg.status.status),msg.status.text)
        rospy.loginfo(text)
 
        if msg.status.status == GoalStatus.ABORTED:
            if self.count<=self.goal_num:
                rospy.loginfo("count num is: %d",self.count)
                os.system(r"/home/tcr/sh_file/clear_costmaps.sh")
                rospy.sleep(1)
                print("send again")
                #print(send_goal)
                goal=send_goal.goal
                self.move_base.send_goal(goal)
                self.move_base.wait_for_result(rospy.Duration(60))
                self.count=self.count+1               
            else:
                rospy.loginfo("goal can not be reached,please check the AGV!") 
        if msg.status.status == GoalStatus.SUCCEEDED:
            self.count=0;
        if self.count==self.goal_num:
            rospy.loginfo("goal can not be reached,please check the AGV!")
    def GoalCallback(self,resend_goal):
        #send_goal=MoveBaseGoal()
        global send_goal
        send_goal=resend_goal
        print(send_goal)
           
if __name__ == '__main__':
    """ main """
    nav_state = NavState()  
    rospy.spin()
